#include "debug.h"
#include "../board.h"

/* COMM */
#include "../comm/suivi_gps.h"
#include "../comm/xbee_comm.h"
#include "../comm/telecommande.h"
/* TASKS */
#include "../tasks/init.h"
#include "../tasks/gestion_vol.h"
#include "../tasks/capteurs.h"
#include "../tasks/eeprom.h"
#include "../tasks/asserv.h"
#include "../tasks/imu.h"
/* UTILS */
#include "../utils/generic.h"
#include "../utils/timing.h"
#include "../utils/can.h"
#include "../utils/trames.h"
#include "../utils/ppm.h"

void BoucleDebug()
{
    //readEEPROM(&coeffsPID, EEPROM_BASE, MAX_PID * sizeof(COEFFS_PID));
    //readEEPROM(&telecommandeOffsets, EEPROM_BASE + EEPROM_OFFSET_TRIMS, sizeof(TELECOMMANDE_DATA));
    //readEEPROM(&gAnglesOffsets, EEPROM_BASE + EEPROM_OFFSET_ANGLES, sizeof(IMU_ANGLES_VITESSES));
    //Teste si les données stockées en EEPROM sont valides : on vérifie que le premier coefficient PID est différent de 0xFFFFFFFF (NaN)
    if (*((long*)(&coeffsPID[0].kp)) == 0xFFFFFFFF)
    {
        //initEEPROM();
        ZeroPID();
    }

    //printf("M. ERRINT: R=%.1f\tP=%.1f\tY=%.1f\n", p_err_int.roulis, p_err_int.tangage, p_err_int.lacet);
    while(1)
    {
        //Traite les commandes reçues par le XBee
        traiterCommandes();
        if(MODE_FAILSAFE_URGENCE)
        {
            PilotageFailsafe();
            continue;
        }
        if(!MODE_DEBUG)
            break;
        if(ENABLE_CAPTEURS)
        {
            if(capteursMAJ)
            {
                capteursMAJ=0;
                /*
                // *** LECTURE DAC ***
                ADCON1bits.SAMP = 1;
                while(!ADCON1bits.SAMP);
                ConvertADC12();
                while(ADCON1bits.SAMP);
                while(!BusyADC12());
                while(BusyADC12());

                printf("ADC : %5.2f V\n",(float) ReadADC12(0) / 2048.0 * 5.0);
                capteurs.jauge = (unsigned int)((float) ReadADC12(0) / 2048.0 * 5000.0);

                printf ("Ab(m) : %d Au(cm) : %d P(cm/s) : %d GPSx : %lu GPSy : %lu J : %u\r\n",
                    capteurs.altitude_baro,
                    capteurs.altitude_us,
                    capteurs.vitesse,
                    capteurs.gps_x,
                    capteurs.gps_y,
                    capteurs.jauge);
                //Pass-through : renvoie la trame modifiée à la station au sol
                envoiTrame (TYPE_TRAME_CAPTEURS, (void*)&capteurs, sizeof(CAPTEURSDATA));

                float vitesse_kmh = capteurs.vitesse * 0.036;
                printf("Vitesse(km/h) : %.2f\t\n", vitesse_kmh);
                //*/
                printf("[i] Etat capteurs\n");
                // Etat ON/OFF des capteurs
                AfficherEtatCapteurs();
                // valeurs calculées à partir des signaux capteurs
                AfficherValeursCapteurs();
            }
        } // endif(capteursMAJ)
        if(anglesMAJ)
        {
            CalculerAngles();
            anglesMAJ=0;
        }
        if(TimeoutHeartbeat())
        {
            heartbeat();
        }
        AttendreMs(5);
        if(!TimeoutIMU())
        {
            PilotageSemiAuto();
        }
        else
        {
            //printf("[!] Timeout IMU! Reset de l'UART2\n");
            U2MODEbits.UARTEN = 0;        // Reset UART2
            U2MODEbits.UARTEN = 1;
            //InitIMU();
        }
    } // end while(1)
}

void ExecuterCommande(unsigned char id)
{
    int i,j;
    char buffer_ENABLE_HEARTBEAT;
    switch(id)
    {
        case '0':
            printf("[C] Reinitialisation du programme");
            gTimer = 0;
            InitProcesseur(); // Initialisation du programme DSPIC
            TestResets(); //Test de la cause de reset // JW: what?
            InitFlagsMAJ();
            InitTelecommande(); // JW: initialisation des min et max de la télécommande
            InitHistTelecommande();
            InitExtremaPPM();
            InitCommandes();
            InitHistAngles();
            ZeroPID();
            InitAngles();
            ZeroErreursIntegrales();
            break;
        case 'i':
            printf("[C] Angles:\nR=%.3f ; P=%.3f ; Y=%.3f\n", RoulisCorrige(), TangageCorrige(), LacetCorrige());
            printf("[C] Vitesses:\nVR=%.3f ; VP=%.3f ; VY=%.3f\n", VitesseAngulaire(ID_ROULIS), VitesseAngulaire(ID_TANGAGE), VitesseAngulaire(ID_LACET));
            printf("[C] Accelerations:\nAR=%.3f ; AP=%.3f ; AY=%.3f\n", AccelAngulaire(ID_ROULIS), AccelAngulaire(ID_TANGAGE), AccelAngulaire(ID_LACET));
            printf("[C] Accelerations:\nAX=%.3f ; AY=%.3f ; AZ=%.3f\n", angles.accelx, angles.accely, angles.accelz);
            break;
        case 'x':
            printf("[C] Accelerations:\nX=%.3f ; Y=%.3f ; Z=%.3f\n", angles.accelx, angles.accely, angles.accelz);
            break;
        case 'b':
            printf("[C] %% Batterie: %f %%\n", PourcentageBatterie());
            break;
        case 'a':
            ReglerOffsetsAngles();
            printf("[C] offsets des angles regles\n");
            break;
        case 't':
            ReglerOffsetsTelecommande();
            printf("[C] offsets telecommande regles\n");
            break;
            printf("[C] gAnglesOffsets :\nR%.3f\nT%.3f\nL%.3f\n",
                    (double) gAnglesOffsets.roulis,
                    (double) gAnglesOffsets.tangage,
                    (double) gAnglesOffsets.lacet);
            break;
        case 'c':
            printf("[C] Consignes moteurs/gouvernes\n");
            printf("MG: %d ; MD: %d\nGG: %d ; GD: %d\n",
                   M_GAUCHE, M_DROIT, G_GAUCHE, G_DROITE);
            break;
        case '+':
            printf("[C] Incrementer consignes gouvernes\n");
            G_GAUCHE = G_GAUCHE +10;
            G_DROITE = G_DROITE +10;
            printf("GG: %d ; GD: %d\n", G_GAUCHE, G_DROITE);
            break;
        case '-':
            printf("[C] Decrementer consignes gouvernes\n");
            G_GAUCHE = G_GAUCHE -10;
            G_DROITE = G_DROITE -10;
            printf("GG: %d ; GD: %d\n", G_GAUCHE, G_DROITE);
            break;
        case '*':
            printf("[C] Incrementer consignes moteurs\n");
            M_GAUCHE = M_GAUCHE +10;
            M_DROIT = M_DROIT +10;
            printf("MG: %d ; MD: %d\n", M_GAUCHE, M_DROIT);
            break;
        case '/':
            printf("[C] Decrementer consignes moteurs\n");
            M_GAUCHE = M_GAUCHE -10;
            M_DROIT = M_DROIT -10;
            printf("MG: %d ; MD: %d\n", M_GAUCHE, M_DROIT);
            break;
        case 'm':
            if((gTimer - lastAnglesMAJ) > ANGLES_TIMEOUT)
                printf("[!] TimeOut centrale\n");
            printf("[C] Matrice d'inertie:\n");
            printf("%d\t%d\t%d\n", angles.m11, angles.m12, angles.m13);
            printf("%d\t%d\t%d\n", angles.m21, angles.m22, angles.m23);
            printf("%d\t%d\t%d\n", angles.m31, angles.m32, angles.m33);
            break;
        case 'l':
            printf("[C] Telecommande:\n");
            printf("ALT   ROU   PUI   TAN         MSA   MAu\n");
            printf("      Gdr   Mdr   Mga   Gga            \n");
            printf("-------------- MOYENNE ----------------\n");
            for(j=0; j < NB_VOIES_TELECOMMANDE; j++)
                printf ("%3d   ", VoieTelecommande(j));
            printf("\n");
            for(j=0; j < NB_VOIES_TELECOMMANDE; j++)
                printf ("%3d%%  ", TelecommandeVersPourcent(j));
            printf("\n--------------------------------\n");
            break;
        case 'u':
            ENABLE_HEARTBEAT = !ENABLE_HEARTBEAT;
            if(ENABLE_HEARTBEAT)
                printf("[C] heartbeat ON\n");
            else
                printf("[C] heartbeat OFF\n");
            break;
        case 'p':
            buffer_ENABLE_HEARTBEAT = ENABLE_HEARTBEAT; // on sauvegarde l'etat de ENABLE_HEARTBEAT pour le restaurer plus tard
            ENABLE_HEARTBEAT = 0; // on désactive le heartbeat pour clarifier l'output
            EnvoyerTramePID();
            ENABLE_HEARTBEAT = buffer_ENABLE_HEARTBEAT;
            break;
        case 'f':
            MODE_FAILSAFE_URGENCE = 1;
            PilotageFailsafe();
            printf("FAILSAFE URGENCE!\n");
            break;
        case 'F':
            MODE_FAILSAFE_URGENCE = 0;
            printf("[i] Sortie failsafe urgence\n");
            break;
        case 's':
            printf("[c] Consignes actuelles:\n");
            
            printf("[i] ROLL: %.1f rad = %.1f%%\n", TelecommandeVersConsigne(V_ROULIS, -PI, PI), TelecommandeVersConsigne(V_ROULIS, -50.0, 50.0));
            printf("[i] PITCH: %.1f rad = %.1f%%\n", TelecommandeVersConsigne(V_TANGAGE, -PI/6, PI/6), TelecommandeVersConsigne(V_TANGAGE, -50.0, 50.0));
            printf("[i] YAW: %.1f rad = %.1f%%\n", TelecommandeVersConsigne(V_LACET, -PI/6, PI/6), TelecommandeVersConsigne(V_LACET, -50.0, 50.0));
            printf("[i] PUISS: %.1f = %.1f%%\n", TelecommandeVersConsigne(V_PUISSANCE, 0.0, BORNE_PUISSANCE), TelecommandeVersConsigne(V_PUISSANCE, 0.0, 100.0));
            break;
        case 'h':
            for(i=0; i < 5; i++)
            {
                printf("[%d] (%ld) R:%.3f\tP:%.3f\tY:%.3f\n",
                       i, gHistAngles[i].date,
                       gHistAngles[i].roulis,
                       gHistAngles[i].tangage,
                       gHistAngles[i].lacet);
                printf("[%d] (%ld) VR:%.3f\tVP:%.3f\tVY:%.3f\n",
                       i, gHistAngles[i].date,
                       gHistAngles[i].vit_roulis,
                       gHistAngles[i].vit_tangage,
                       gHistAngles[i].vit_lacet);
                printf("[%d] (%ld) AR:%f\tAP:%f\tAY:%f\n",
                       i, gHistAngles[i].date,
                       gHistAngles[i].acc_roulis,
                       gHistAngles[i].acc_tangage,
                       gHistAngles[i].acc_lacet);
                printf("--------------------\n");
            }
            break;
        default:
            printf("[!] Commande inconnue: %c\n", id);
            break;
    }
}

void TestResets()
{
    if(RCONbits.EXTR)
		printf("[i] Reset externe (MCLR)\n");
	if(RCONbits.BOR)
    {
		if(RCONbits.POR)
			printf("[i] Reset power-on\n");
		else
			printf("[i] Reset brown-out\n");
	}
	if(RCONbits.SWR)
		printf("[i] Software reset\n");
	if(RCONbits.TRAPR)
		printf("[i] Trap reset\n");
	if(RCONbits.IOPUWR)
		printf("[i] Illegal Operation reset\n");
	RCON = 0;
}
